﻿
// 2020/1/16: 首个记录，基于V3 2019/10/11
// 2020/5/15: 移除编译warning
// 2020/6/17: public ID改为raw ID
// 2020/7/20: 更新样本标题
// 2020/9/29: 修正TTLC计算
// 2020/11/12: 支持Spadas7
// 2022/11/2: 支持Spadas8.4
// 2023/10/11: 补充部分英文注释
// 2024/7/3: 基于新样本框架。枚举改为使用enum class
// 2025/3/31: 实现插值
// 2025/8/4: 不使用cat宏

#ifndef LANE_SENSOR_SAMPLE_V4_H
#define LANE_SENSOR_SAMPLE_V4_H

#include "spadas.h"

namespace lane_sensor_sample_v4
{
	using namespace spadas;

	enum class LineDescription
	{
		Model = 1, // Describe with model coefficients and consistent properties / 通过模型参数和一致的属性进行描述
		Segments = 2, // Describe with many segments and properties of each segment / 通过多段线段和各自的属性进行描述
		Both = 3, // Both model and segments are supported / Model和Segments双模式描述
	};

	enum class LineClass
	{
		General = 1, // 未指定
		Dashed = 2, // 虚线
		Solid = 3, // 实线
		DoubleDashed = 4, //双虚线
		DoubleSolid = 5, //双实线
		SolidDashed = 6, // 左实右虚
		DashedSolid = 7, // 左虚右实
		Curb = 8, // 路沿
		Guardrail = 9, // 护栏
		Barrier = 10, //障碍物边界
	};

	enum class LineColor
	{
		Unknown = 1, // 未知
		White = 2, // 白色
		Yellow = 3, // 黄色
		Red = 4, // 红色
		Blue = 5, // 蓝色
		Orange = 6, // 橙色
		Green = 7, // 绿色
	};

	struct LineSegmentSolidProperty
	{
		Bool valid; // Whether it's valid, FALSE indicates virtual part / 是否有效，FALSE表示为虚线段
		LineColor color; // Color of the line segment / 线段颜色
		OptionalDouble width; // [m] Width of the line segment / 线段宽度

		LineSegmentSolidProperty()
		{
			valid = FALSE;
			color = LineColor::Unknown;
		}
	};

	struct LineSegment
	{
		Int id; // ID of line segment / 线段ID
		Point2D p1; // [m] x-y coordination of the start point / 线段首端点的x-y坐标
		OptionalDouble z1; // [m] z coordination of the start point / 线段首端点的z坐标
		Point2D p2; // [m] x-y coordination of the end point / 线段末端点的x-y坐标
		OptionalDouble z2; // [m] z coordination of the end point / 线段末端点的z坐标
		LineSegmentSolidProperty solidProperty; // Properties of solid line segment (Not for virtual part) / 实线段的属性（虚线段则为空）
	};

	struct LaneLine
	{
		// Basic info / 基本信息
		Int id; // ID of the lane line, zero as invalid / 车道线ID，0表示无效
		OptionalInt rawID; // Raw ID of the lane line / 车道线原始ID
		OptionalDouble confidence; // [%] Confidence of the lane line / 车道线置信度
		LineDescription description; // How to describe the lane line's curve / 车道线的描述方式

		// Model description / 模型描述
		LineClass clas; // Model description: Classification of lane line / Model模式：一致性车道线类型
		LineColor color; // Model description: Color of lane line / Model模式：一致性车道线颜色
		OptionalDouble width; // [m] Model description: Line width of lane line / Model模式：一致性车道线宽度
		Double param[4]; // Model description / Model模式：y = coef[0] + coef[1] * x + coef[2] * x^2 + coef[3] * x^3
		Bool paramZValid; // Model description: Whether z-function is valid / Model模式：z方程是否有效
		Double paramZ[4]; // Model description / Model模式：z = coef[0] + coef[1] * x + coef[2] * x^2 + coef[3] * x^3
		OptionalDouble rearEnd; // [m] Model description: X-axis coordination of read end of the lane line / Model模式：曲线后端点位置(x轴)
		OptionalDouble frontEnd; // [m] Model description: X-axis coordination of front end of the lane line / Model模式：曲线前端点位置(x轴)

		// Segment description / 线段描述
		Array<LineSegment> segments; // Segment description: List of line segments / Segments模式：线段列表

		LaneLine()
		{
			id = -1;
			description = LineDescription::Model;
			clas = LineClass::General;
			color = LineColor::Unknown;
			param[0] = param[1] = param[2] = param[3] = 0;
			paramZValid = FALSE;
			paramZ[0] = paramZ[1] = paramZ[2] = paramZ[3] = 0;
		}

		Bool modelValid()
		{
			return description == LineDescription::Model || description == LineDescription::Both;
		}

		Bool segmentsValid()
		{
			return description == LineDescription::Segments || description == LineDescription::Both;
		}
	};

	struct LaneSensorSampleData
	{
		// Lane lines / 车道线
		Array<LaneLine> lines; // List of lane lines / 车道线列表
		OptionalInt firstLeftLine; // Index of the first lane line to the left side / 左侧第一条车道线序号
		OptionalInt firstRightLine; // Index of the first lane line to the right side / 右侧第一条车道线序号
		OptionalInt secondLeftLine; // Index of the second lane line to the left side / 左侧第二条车道线序号
		OptionalInt secondRightLine; // Index of the second lane line to the right side / 右侧第二条车道线序号

		// Lane properties / 道路属性部分
		OptionalDouble laneWidth; // [m] Current lane's width / 当前车道宽度
		OptionalDouble laneHeading; // [deg] Current lane's orientation / 当前车道朝向
		OptionalDouble laneCurvature; // [1/m] Current lane's curvature / 当前车道曲率

		// Driving status along current lane / 本车道行驶状态
		OptionalDouble lateralVelocity; // [m/s] Lateral speed to current lane / 当前车道横向速度
		OptionalDouble centerDeparture; // [m] Central departure to current lane / 当前车道中央偏离距离

		// Subject vehicle info / 车辆参数部分
		OptionalDouble vehiVX; // [kph] Subject vehicle's speed / 本车车速
		OptionalDouble vehiCurv; // [1/m] Subject vehicle's turning curvature / 本车行驶曲率
		OptionalDouble vehiAX; // [m/s²] Subject vehicle's longitudinal acceleration / 本车纵向加速度
		OptionalDouble vehiWidth; // [m] Subject vehicle's width / 本车宽度
		OptionalDouble vehiOverhang; // [m] Subject vehicle's front overhang / 本车前悬
		OptionalDouble vehiWheelBase; // [m] Subject vehicle's wheel base / 本车轴距

		// Sensor detect range / 车道线识别边界
		Double rearBound; // [m] Rear bound of detect range / 传感器检测范围后边界
		Double frontBound; // [m] Front bound of detect range / 传感器检测范围前边界

		LaneSensorSampleData()
		{
			rearBound = 0;
			frontBound = 30;
		}

		OptionalDouble getDTLC(LaneLine& line)
		{
			if (line.modelValid() && vehiWidth.valid && vehiOverhang.valid)
			{
				Double *values = line.param;
				Bool isLeft = values[0] > 0;

				Double x = -1 * vehiOverhang.value;
				Double y = (values[3] * x * x * x + values[2] * x * x
					+ values[1] * x + values[0]) * (isLeft ? 1 : -1);
				Double cosTheta = math::abs(math::cos(math::atan(values[1])));
				return cosTheta * (y - vehiWidth.value / 2.0);
			}
			else return OptionalDouble();
		}

		OptionalDouble getSTLC(LaneLine& line)
		{
			if (line.modelValid() && vehiVX.valid)
			{
				Double *values = line.param;
				Bool isLeft = values[0] > 0;

				Double curv = vehiCurv.valid ? vehiCurv.value : 0;
				Double fo = vehiOverhang.valid ? vehiOverhang.value : 0.9;
				Double wb = vehiWheelBase.valid ? vehiWheelBase.value : 2.8;

				Double sinTheta = math::sin(math::atan(values[1] - curv * (fo + wb)));
				return (isLeft ? -1.0 : 1.0) * sinTheta * vehiVX.value / 3.6;
			}
			else return OptionalDouble();
		}

		OptionalDouble getTTLC(LaneLine& line)
		{
			if (line.modelValid() && vehiVX.valid && vehiWidth.valid && vehiOverhang.valid)
			{
				if (vehiVX.value <= 0) return OptionalDouble();

				Double *values = line.param;
				Bool isLeft = values[0] > 0;

				Double wb = vehiWheelBase.valid ? vehiWheelBase.value : 2.8;
				Double x0 = -vehiOverhang.value - wb;
				Double x = x0;
				Double y = 0;
				Double vx = vehiVX.value / 3.6;

				UInt loopTimes = vx < 5 ? 1000 : (UInt)(50.0 / vx * 100); // No output if crossing beyond 50m (Can't guarantee precision) / 超过50米越线则不输出（精度无法保证）

				Double rx = wb;
				Double ry = (isLeft ? 0.5 : -0.5) * vehiWidth.value;

				for (UInt i = 0; i <= loopTimes; i++)
				{
					Double k = 0;
					if (vehiCurv.valid) k = vehiCurv.value * (x - x0);

					Double cosa = 1.0 / math::abs(1 + k);
					Double sina = cosa * k;

					Double tx = x + cosa * rx - sina * ry;
					Double ty = y + sina * rx + cosa * ry;
					Double ly = values[3] * tx * tx * tx + values[2] * tx * tx + values[1] * tx + values[0];

					Bool passed = FALSE;
					if (isLeft)
					{
						if (ly <= ty) passed = TRUE;
					}
					else
					{
						if (ly >= ty) passed = TRUE;
					}
					if (passed)
					{
						if (i == 0) return OptionalDouble();
						else return (Double)i / 100;
					}

					if (vehiAX.valid) vx += vehiAX.value * 0.01;
					if (vx <= 0) break;

					x += vx * 0.01 * cosa;
					y += vx * 0.01 * sina;
				}

				return OptionalDouble();
			}
			else return OptionalDouble();
		}
	};

	class LaneSensorSample : public Object<class LaneSensorSampleVars>
	{
	public:
		SPADAS_TYPE("lane_sensor_sample_v4.LaneSensorSample")

		LaneSensorSample();
		FullTimestamp& timestamp();
		LaneSensorSampleData *operator ->();

		static String getProtocolName(UInt channel);
		SessionGeneralSample toGeneralSample();

		static Array<String> getProtocolNames(UInt channel);
		Bool fromGeneralSample(String protocol, SessionGeneralSample generalSample);
	};
}

#endif
